#include "PIDController.hpp"
#include "stdio.h"

/************
 *  @fn     PIDController::PIDController()
 *  @brief  PID控制器的无参构造函数，默认控制器所有状态与参数均为0
 *  @param  无
 */
PIDController::PIDController(){
    this->kp = 0.0;
    this->ki = 0.0;
    this->kd = 0.0;
    this->last_error = 0.0;
    this->integral_error = 0.0;
}

/************
 *  @fn     PIDController::PIDController(double kp,double ki,double kd)
 *  @brief  一般式PID控制器的构造函数
 *  @param  kp  比例系数
 *  @param  ki  积分系数
 *  @param  kd  微分系数
 */
PIDController::PIDController(double kp,double ki,double kd){
    this->kp = kp;
    this->ki = ki;
    this->kd = kd;
    this->last_error = 0.0;
    this->integral_error = 0.0;
}

/************
 *  @fn     double PIDController::getKp(void)
 *  @brief  获取PID控制器的比例系数
 *  @param  无
 *  @return PID控制器的比例系数
 */
double PIDController::getKp(void){
    return this->kp;
}

/************
 *  @fn     void PIDController::setKp(double kp)
 *  @brief  设置PID控制器的比例系数
 *  @param  kp  PID控制器的比例系数
 *  @return 无
 */
void PIDController::setKp(double kp){
    this->kp = kp;
}

/************
 *  @fn     double PIDController::getKi(void)
 *  @brief  获取PID控制器的积分系数
 *  @param  无
 *  @return PID控制器的积分系数
 */
double PIDController::getKi(void){
    return this->ki;
}

/************
 *  @fn     void PIDController::setKi(double ki)
 *  @brief  设置PID控制器的积分系数
 *  @param  ki  PID控制器的积分系数
 *  @return 无
 */
void PIDController::setKi(double ki){
    this->ki = ki;
}

/************
 *  @fn     double PIDController::getKd(void)
 *  @brief  获取PID控制器的微分系数
 *  @param  无
 *  @return PID控制器的微分系数
 */
double PIDController::getKd(void){
    return this->kd;
}

/************
 *  @fn     void PIDController::setKd(double kd)
 *  @brief  设置PID控制器的微分系数
 *  @param  kd  PID控制器的微分系数
 *  @return 无
 */
void PIDController::setKd(double kd){
    this->kd = kd;
}

/************
 *  @fn     double PIDController::getIntegralTime(void)
 *  @brief  获取PID控制器的积分时间常数
 *  @param  无
 *  @return PID控制器的积分时间常数
 */
double PIDController::getIntegralTime(void){
    return (this->kp / this->ki);
}

/************
 *  @fn     void PIDController::setIntegralTime(double tau_i)
 *  @brief  设置PID控制器的积分时间常数
 *  @param  tau_i  PID控制器的积分时间常数
 *  @return 无
 */
void PIDController::setIntegralTime(double tau_i){
    this->ki = (this->kp / tau_i);
}

/************
 *  @fn     double PIDController::getDifferentialTime(void)
 *  @brief  获取PID控制器的微分时间常数
 *  @param  无
 *  @return PID控制器的微分时间常数
 */
double PIDController::getDifferentialTime(void){
    return (this->kd / this->kp);
}

/************
 *  @fn     void PIDController::setDifferentialTime(double tau_d)
 *  @brief  设置PID控制器的微分时间常数
 *  @param  tau_d  PID控制器的微分时间常数
 *  @return 无
 */
void PIDController::setDifferentialTime(double tau_d){
    this->kd = (this->kp * tau_d);
}


/************
 *  @fn     double PIDController::iterate(double error)
 *  @brief  PID控制器迭代计算
 *  @param  error   系统当前误差
 *  @return PID控制器的输出控制量
 */
double PIDController::iterate(double error){
    static int count = 0;
    if (error == 0) {
        count ++;
        if(count > 10000){
            count = 0;
            this->integral_error = 0;
        }
        return 0;
    }

    double output = this->kp * error 
                  + this->ki * this->integral_error 
                  + this->kd * (error - this->last_error);
    error = error / 2;
    this->integral_error += error;
    if( this->integral_error > 10) integral_error = 10;
    if( this->integral_error < -10) integral_error = -10;
    this->last_error = error;
    if( output > 10 ) output = 10;
    if( output < -10 ) output = -10;
    return output;
}

void PIDController::debugPrint(void){
    printf("PID:kp=%.2lf,ki=%.2lf,kd=%.2lf\r\nlast_error=%.2lf,integral_error=%.2lf\r\n",kp,ki,kd,last_error,integral_error);
}

